#! /usr/bin/env python

# this ROS node converts a PolygonStamped (such as might be produced by
# the color based tracking) and produces a Convex_Space.  You must pass
# the camera parameters on the command line.

import roslib
roslib.load_manifest('intersector')
import rospy

from geometry_msgs.msg import   Point
from intersector.msg   import Convex_Space
from visualization_msgs.msg import Marker

import sys   # for argv



def input_handler(space):
    global marker_color
    global marker_pub
    global matches

    for str in matches:
        if space.aux_payload.find(str) == -1:
#            print "FAIL: str='%s' aux_payload='%s'" % (str, space.aux_payload)
            return

#    print space
#    print

    marker = Marker()
    marker.header.frame_id = "/map"
    marker.header.stamp    = rospy.Time()
    marker.ns = ""
    marker.id = 0
    marker.type   = Marker.POINTS
    marker.action = Marker.ADD
    marker.pose.position.x = 0
    marker.pose.position.y = 0
    marker.pose.position.z = 0
    marker.pose.orientation.x = 0
    marker.pose.orientation.y = 0
    marker.pose.orientation.z = 0
    marker.pose.orientation.w = 0
    marker.scale.x = .1
    marker.scale.y = .1
    marker.scale.z = .1
    marker.color = marker_color

    marker.points = []

    # this line cuts off anything before the string "convex_set: " in the aux_payload
    convex_set = space.aux_payload[ space.aux_payload.find("convex_set: ") :]

    # this line cuts off that string at the next newline character
    convex_set = convex_set.split('\n')[0]

    # this line splits the string into pieces delineated by spaces
    convex_set = convex_set.split(' ')

#    print convex_set

    for candidate in convex_set:
        if len(candidate) == 0 or candidate[0] != '(':
            continue
        coords = candidate[1:-1].split(',')
#        print coords

        marker.points.append(Point(float(coords[0]), float(coords[1]), float(coords[2])))
#        print marker.points[-1]

#    print

#    print marker.points
#    print

    marker_pub.publish(marker)



if __name__ == '__main__':
    global marker_color
    global marker_pub
    global matches

    input_topic  = sys.argv[1]
    output_topic = sys.argv[2]

    marker_color = Marker().color
    marker_color.a = 1
    marker_color.r = float(sys.argv[3])
    marker_color.g = float(sys.argv[4])
    marker_color.b = float(sys.argv[5])

    matches = []
    for arg in sys.argv[6:]:
        if arg[:6] == "MATCH=":
            matches.append(arg[6:])

#    print "matches = %s" % matches


    rospy.init_node('mark_intersections')

    marker_pub = rospy.Publisher(output_topic, Marker)
    rospy.Subscriber(input_topic, Convex_Space, input_handler)

    rospy.spin()

